#ifndef WALKAWARE_SERVICES_POINTCLOUDSERVICE_H
#define WALKAWARE_SERVICES_POINTCLOUDSERVICE_H

#include <QMutexLocker>

#include <WalkaWare/Shared/PointCloudExtractor.h>
#include <WalkaWare/Services/PollingService.h>

namespace WalkaWare
{

namespace Services
{

class PointCloudService : public PollingService
{
	Q_OBJECT

	public:

		typedef PollingService    BaseType;
		typedef PointCloudService ThisType;

		PointCloudService(void)
			: inTimestamp (0)
			, inReady     (false)
		{
			;
		}

		virtual ~PointCloudService(void)
		{
			this->quit();
		}

		void loadCalibration(const char * fileName)
		{
			const bool running = this->isRunning();
			if (running)
			{
				this->blockSignals(true);
				this->quit();
			}

			this->extractor.loadCalibration(fileName);

			if (running)
			{
				this->start();
				this->blockSignals(false);
			}
		}

	signals:

		void pointCloudReady(WalkaWare::Shared::PointCloud pointCloud, quint32 timestamp);

	public slots:

		void extract(WalkaWare::Shared::RawDepthMap depthMap, quint32 timestamp)
		{
			if (!this->isRunning()) return;
			this->pushDepthMap(depthMap, timestamp);
		}

	protected:

		Shared::PointCloudExtractor extractor;
		Shared::RawDepthMap         inDepthMap;
		quint32                     inTimestamp;
		bool                        inReady;

		void pushDepthMap(const Shared::RawDepthMap & depthMap, quint32 timestamp)
		{
			{
				QMutexLocker(&(this->sMutex));
				if (this->end) return;
				this->inDepthMap  = depthMap;
				this->inTimestamp = timestamp;
				this->inReady     = true;
			}
			this->sCondition.wakeAll();
		}

		bool popDepthMap(Shared::RawDepthMap & depthMap, quint32 & timestamp)
		{
			bool ended = false;
			this->sMutex.lock();
			while (!this->end && !this->inReady)
			{
				this->sCondition.wait(&(this->sMutex));
			}
			ended = this->end;
			if (!ended)
			{
				depthMap  = this->inDepthMap;
				timestamp = this->inTimestamp;
				this->inReady = false;
			}
			this->sMutex.unlock();
			return !ended;
		}

		virtual void run(void)
		{
			Shared::RawDepthMap depthMap;
			quint32             timestamp = 0;

			while (this->popDepthMap(depthMap, timestamp))
			{
				Shared::PointCloud pointCloud;
				if (!this->extractor.extract(depthMap, pointCloud)) continue;
				emit pointCloudReady(pointCloud, timestamp);

			}
		}

	private:

		Q_DISABLE_COPY(PointCloudService)
};

} // end namespace Services

} // end namespace WalkaWare

#endif // WALKAWARE_SERVICES_POINTCLOUDSERVICE_H
